#!/usr/bin/env python3
# coding=utf-8
import socket
import os
import subprocess
import select
import struct

# Source ROS环境
ros_setup_path = "/opt/ros/noetic/setup.bash"
mavros_ws_setup_path = "/home/orangepi/car_web/devel/setup.bash"
# 动态 source setup.bash
command = ['bash', '-c', f"source {ros_setup_path} && source {mavros_ws_setup_path} && env"]
proc = subprocess.Popen(command, stdout=subprocess.PIPE)
for line in proc.stdout:
    (key, _, value) = line.decode("utf-8").partition("=")
    os.environ[key] = value.strip()

import rospy
from std_msgs.msg import String

# 定义服务器IP和端口
server_ip = '192.168.195.111'
server_port = 6666

# 创建 TCP socket
client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client_socket.setblocking(False)  # 设置为非阻塞模式

# 定义变量来存储接收的数据，初始值全部为0.0，为0.0时为无效值，对于没使用到的值应全部置0.0
get_check1 = 0.0       # 包头（888.0有效）
get_speed = 0.0       # 速度
get_mode = 0.0        # 飞行模式/任务：1.0表示GPS巡航模式，2.0表示飞机控制模式，3.0表示...
get_doit = 0.0        # 执行确认位(1.0表示有效，0.0表示无效)
get_longitude = [0.0,0.0,0.0,0.0]   # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
get_latitude =  [0.0,0.0,0.0,0.0]   # 列表存储纬度
get_altitude =  [0.0,0.0,0.0,0.0]   # 列表存储海拔（高度）
get_check2 = 0.0       # 包尾（666.0有效）

# 定义变量来存储要发送的数据，初始值全部为0.0，为0.0时为无效值，对于没使用到的值应全部置0.0
send_check1 = 888.0     # 包头，888.0有效
send_speed = 0.0
send_mode = 0.0        # 飞行模式/任务：1.0表示...2.0表示...
send_doit = 0.0        # 执行确认位(1.0表示有效，0.0表示无效)
send_longitude = [0.0,0.0,0.0,0.0]   # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
send_latitude =  [0.0,0.0,0.0,0.0]   # 列表存储纬度
send_altitude =  [0.0,0.0,0.0,0.0]   # 列表存储海拔（高度）
send_check2 = 666.0     # 包尾，666.0有效

def send_datas_init():
    global send_check1
    global send_speed
    global send_mode
    global send_doit
    global send_longitude
    global send_latitude
    global send_altitude
    global send_check2
    send_check1 = 888.0     # 包头，888.0有效
    send_speed = 0.0
    send_mode = 0.0        # 飞行模式/任务：1.0表示...2.0表示...
    send_doit = 0.0        # 执行确认位(1.0表示有效，0.0表示无效)
    send_longitude = [0.0,0.0,0.0,0.0]   # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
    send_latitude =  [0.0,0.0,0.0,0.0]   # 列表存储纬度
    send_altitude =  [0.0,0.0,0.0,0.0]   # 列表存储海拔（高度）
    send_check2 = 666.0     # 包尾，666.0有效

def get_datas_to_0():
    global get_check1
    global get_speed
    global get_mode
    global get_doit
    global get_longitude
    global get_latitude
    global get_altitude
    global get_check2
    get_check1 = 0.0     # 包头，888.0有效
    get_speed = 0.0
    get_mode = 0.0        # 飞行模式/任务：1.0表示...2.0表示...
    get_doit = 0.0        # 执行确认位(1.0表示有效，0.0表示无效)
    get_longitude = [0.0,0.0,0.0,0.0]   # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
    get_latitude =  [0.0,0.0,0.0,0.0]   # 列表存储纬度
    get_altitude =  [0.0,0.0,0.0,0.0]   # 列表存储海拔（高度）
    get_check2 = 0.0     # 包尾，666.0有效

# socket发送数据
def send_data(client_socket, data_to_send):
    """发送数据到服务器"""
    try:
        # 使用 struct 打包数据，17个 double 类型
        packed_data = struct.pack('>17d', *data_to_send)
        # 发送打包的数据到服务器
        client_socket.sendall(packed_data)
        print(f"Sent data: {data_to_send}")
    except BlockingIOError:
        print("发送数据时阻塞，请稍后再试。")
    except Exception as e:
        print(f"发送数据时出错: {e}")

# socket接收数据（）
def receive_data(client_socket):
    """从服务器接收数据"""
    try:
        # 使用 select 监听 socket 是否可读
        readable, _, _ = select.select([client_socket], [], [], 0.05)
        if readable:
            # 接收服务器的确认信息
            response = client_socket.recv(136)
            if not response:
                print(f"和服务端断开连接，尝试重新连接服务端...")
                # 连接到服务器
                try:
                    client_socket.connect((server_ip, server_port))
                except BlockingIOError:
                    # 在非阻塞模式下，连接可能会立即失败
                    print("socket连接失败...")
                    pass
            
            # 解包接收到的数据
            try:
                unpacked_data = struct.unpack('>17d', response)
                get_check1, get_speed, get_mode, get_doit, \
                get_longitude[0], get_latitude[0], get_altitude[0], \
                get_longitude[1], get_latitude[1], get_altitude[1], \
                get_longitude[2], get_latitude[2], get_altitude[2], \
                get_longitude[3], get_latitude[3], get_altitude[3],get_check2 = unpacked_data

                print(f"收到来自服务端的数据:")
                print(f"  经度: {get_longitude}")
                print(f"  纬度: {get_latitude}")
                print(f"  高度: {get_altitude}")
                print(f"  速度: {get_speed}")
                print(f"  飞行模式: {get_mode}")
            except struct.error as e:
                print(f"解包数据时发生错误: {e}")
    except BlockingIOError:
        print("接收数据时阻塞，请稍后再试。")
    except Exception as e:
        print(f"接收数据时出错: {e}")

# 回调函数
def callback(data):
    rospy.loginfo("I heard %s", data.data)



if __name__ == "__main__":
    # 连接到服务器
    while True:
        try:
            print("socket连接中...")
            client_socket.connect((server_ip, server_port))
            print("socket连接成功...")
            break
        except BlockingIOError:
            # 在非阻塞模式下，连接可能会立即失败
            print("socket连接失败...")
            pass
    # ros初始化
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("chatter", String, callback)
    rate = rospy.Rate(10)  # 设定频率为10Hz

    while not rospy.is_shutdown():
        # 这里可以放置其他代码或处理逻辑 
        rate.sleep()  # 让循环按照设定的频率休眠（和回调函数的刷新速率无关）

        # 模拟要发送的数据
        send_check1 = 888.0
        send_speed = 1.0       # 速度
        send_mode = 0.0        # 飞行模式/任务：1.0表示...2.0表示...
        send_doit = 1.0        # 执行确认位(1.0表示有效，0.0表示无效)
        #[21,67,63,89,110,92,39,56]
        #21.679217,110.919416飞机的
        #21.679362,110.919185飞机的空旷地
        #21.679412,110.919200车子自己测的
        
        #21.679377  110.919176
        
        #21.679194    110.919415
        send_longitude[0] = 110.919176  # GPS定位1
        send_latitude[0] = 21.679377
        send_altitude[0] = 25
        send_longitude[1] = 110.919176  # GPS定位2
        send_latitude[1] = 21.679377
        send_altitude[1] = 20
        send_longitude[2] = 110.919415# GPS定位3
        send_latitude[2] = 21.679194
        send_altitude[2] = 23
        send_longitude[3] = 110.919176   # GPS定位4
        send_latitude[3] = 21.679377
        send_altitude[3] = 25
        send_check2 = 666.0
        # (校验位(值为888.0有效),速度,飞行模式/任务,执行位(1.0表示有效，0.0表示无效),经度1, 纬度1,高度1,经度2, 纬度2,高度2,经度3, 纬度3,高度3,经度4, 纬度4, 高度4)
        data_to_send = [send_check1, send_speed, send_mode,send_doit,\
                        send_longitude[0], send_latitude[0],send_altitude[0],\
                        send_longitude[1], send_latitude[1],send_altitude[1],\
                        send_longitude[2], send_latitude[2],send_altitude[2],\
                        send_longitude[3], send_latitude[3],send_altitude[3],send_check2]
        # 调用发送函数
        send_data(client_socket, data_to_send)

        # 调用接收函数
        receive_data(client_socket)

        # 假设有个退出机制
        user_input = 'yes'
        if user_input.lower() == 'no':
            print("客户端退出...")
            break



    # python中不需要手动刷新回调函数，回调函数的执行速率取决于发布速率(只要节点不退出)
    # rospy.spin() # 保持节点活跃，有消息时调用回调函数

